/***************************************************************************
 创建者: 华磊
 开始时间: 2019.1.14
 copyright: (C) 华友高科
 ***************************************************************************/
#include "cu1sensoradapter.h"
#include "domparser.h"
#include <QTcpSocket>
#include "cu1positioncalculate.h"
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#define IF_DEBUG 1

Cu1SensorAdapter::Cu1SensorAdapter(QString configFileIn, QObject *parent) : QObject(parent)
{
    isInitialOkStatus=false;
    isConnectedStatus=false;
//    laserRequestCount=0;
    imageRequestCount=0;
    if(1==initialConfigFile(configFileIn))
    {
        if(true==isUseCu1Sensor)
        {
            posCalculate=new Cu1PositionCalculate( distanceOfTwoLaser, distanceOfLaserAndVehicleCenter,
                                                  distanceOffsetOfCamera,ratioOfPix);
            isInitialOkStatus=true;
            clientSocket = new QTcpSocket;
            if(1==connectSensorServer(sensorServerIp,sensorServerPort))
            {
                isConnectedStatus=true;
            }
            timer=new QTimer();
            connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            timer->start( 100 );
        }
        else
        {
            isInitialOkStatus=true;
        }

    }
    else
    {

    }
}

bool Cu1SensorAdapter::isInitialOk()
{
    return isInitialOkStatus;

}

bool Cu1SensorAdapter::isSocketOkStatus()
{
    return isConnectedStatus;

}

bool Cu1SensorAdapter::isUseThisModule()
{
    return isUseCu1Sensor;

}

int Cu1SensorAdapter::getVehiclePosition(PositionInfo &positionOut)
{
    if(false==isInitialOkStatus)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return 0;
    }
    else if(false==isConnectedStatus)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -1;
    }
    else if(false==isLaser1DataOk)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -10;
    }
    else if(false==isLaser1DataOk)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -11;
    }
    else if(imageRequestCount>maxRequestLimit)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -2;
    }
    positionOut.isUpdateSucess=true;
    positionOut.matchScore=100;
    positionOut.maxMatchError=0;
    positionOut.position=positionFeedback;
}

int Cu1SensorAdapter::getRawData(int &requestCountOut, double &l1Out, double &l2Out,
                                 double &imageOffsetOut, bool &isSucessOut, int &errorCodeOut)
{
    requestCountOut=imageRequestCount;
    l1Out= laser1Data;
    l2Out= laser2Data;
    imageOffsetOut= imageOffsetData;
    isSucessOut=isSensorSucess;
    errorCodeOut=sensorErrorCode;
    return 1;
}


void Cu1SensorAdapter::timeOut_slot()
{
    //自动重新连接
    if(false==isConnectedStatus)
    {
        if(1==connectSensorServer(sensorServerIp,sensorServerPort))
        {
            isConnectedStatus=true;
        }
    }


    processReceiveBuff(positionFeedback);
    if(0==imageRequestCount%4)//太长会导致，二维码从不识别到识别的过程太长。太短会导致，相机程序启动失败时，客户程序多个json数据连接到一起
    {
        sendGetAllInfoCommand();
        imageRequestCount++;
    }
    else
    {
        imageRequestCount++;
    }


}

int Cu1SensorAdapter::connectSensorServer(QString ip, int port)
{
//        qDebug() << " +++++++++connected flag:"
//                 << connect(tmpClientSocket,SIGNAL(error(QAbstractSocket::SocketError)),
//                            this,SLOT(socketError_slot(QAbstractSocket::SocketError)),Qt::DirectConnection);
        clientSocket->abort();
        clientSocket->connectToHost(ip,port);
        int waitTime=300;//太长会卡住界面和网页访问
        if(!clientSocket->waitForConnected(waitTime))
        {
            // 登录失败
            qDebug() << "Cu1SensorAdapter::connectSensorServer 登录失败:" << clientSocket->errorString();
            isConnectedStatus = false;
            return -1;
        }
        isConnectedStatus = true;

    return 1;
}

int Cu1SensorAdapter::initialConfigFile(QString configFileName)
{
    qDebug() << "Cu1SensorAdapter::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile ==== 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseCu1Sensor", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseCu1Sensor);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "sensorServerIp", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, sensorServerIp);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "sensorServerPort", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, sensorServerPort);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "distanceOfTwoLaser", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, distanceOfTwoLaser);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "distanceOfLaserAndVehicleCenter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, distanceOfLaserAndVehicleCenter);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "distanceOffsetOfCamera", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, distanceOffsetOfCamera);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "ratioOfPix", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, ratioOfPix);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "maxRequestLimit", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, maxRequestLimit);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "laserSensorDirection", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, laserSensorDirection);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "laserSensorMaxRange", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "Cu1SensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, laserSensorMaxRange);




    domparser.closeXml();
    return 1;

}

int Cu1SensorAdapter::sendGetAllInfoCommand()
{

    //json 生成
    QJsonObject obj_root;
    QJsonObject obj_parameter;
//    obj_parameter.insert("program_file_name", QString::fromStdString(fileName));
    obj_root.insert("cmd", QJsonValue("getalldata"));
    obj_root.insert("param", obj_parameter);
    obj_root.insert("passWord", QJsonValue("123456"));
    QJsonDocument jsonDoc(obj_root);
    QByteArray tmpString=jsonDoc.toJson();
    tmpString+="\r\n";
//    qDebug()<<"sendGetAllInfoCommand tmpString="<<tmpString;

    //socket发送
    if(clientSocket->state() != QTcpSocket::ConnectedState)
    {
        isConnectedStatus = false;
        qDebug() << "Cu1SensorAdapter::sendGetAllInfoCommand error, socket 未连接:" << clientSocket->errorString();
        return -1;
    }
    clientSocket->write(tmpString);
    clientSocket->flush();
    return 1;

}

int Cu1SensorAdapter::processReceiveBuff(VehiclePosition &posOut)
{

    //socket接收
    if(clientSocket->state() != QTcpSocket::ConnectedState)
    {
        isConnectedStatus = false;
        qDebug()<<"Cu1SensorAdapter::processReceiveBuff error,clientSocket->isValid()==false ";
        return -1;
    }
    QByteArray receiveByteArry = clientSocket->readAll();
    clientSocket->flush();
    if(0==receiveByteArry.size())
    {
        return -1;
    }
    //如果有多个json，则只取第一个json
    QString tmpStr=receiveByteArry;
    QString resultStr=tmpStr.left(tmpStr.indexOf("\r\n"));

    //json处理
    QJsonParseError jsonError;
    QJsonDocument receiveJsonDoc = QJsonDocument::fromJson(resultStr.toLatin1(), &jsonError);

    if(jsonError.error != QJsonParseError::NoError || true==receiveJsonDoc.isNull() )
    {   //多个json数据在一个字符串里面时，解析不出来。
          qDebug() << "json error,"<<receiveJsonDoc<<receiveByteArry;
          return -1;
    }
    QJsonObject redeiveJsonObj = receiveJsonDoc.object();

    //输出所有key，这一步是非必须的，放最后的话，你可能读不到任何key
    QStringList keys = redeiveJsonObj.keys();
    for(int i = 0; i < keys.size(); i++)
    {
//        qDebug() << "key" << i << " is:" << keys.at(i);
    }

    //因为是预先定义好的JSON数据格式，所以这里可以这样读取
    if(redeiveJsonObj.contains("issuc"))
    {
        SensorRawData dataOut;
        if(1==process_manual_allSensorData(redeiveJsonObj,dataOut))
        {
            laser1Data=laserSensorDirection*dataOut.d1/1000.0;
            laser2Data=laserSensorDirection*dataOut.d2/1000.0;
            imageOffsetData=dataOut.offsetx/1000.0;
            isSensorSucess=dataOut.issuc;
            sensorErrorCode=dataOut.errcode;
            if(laser1Data<laserSensorMaxRange && laser1Data>-laserSensorMaxRange)
            {
                isLaser1DataOk=true;
            }
            else
            {
                isLaser1DataOk=false;
            }

            if(laser2Data<laserSensorMaxRange && laser2Data>-laserSensorMaxRange)
            {
                isLaser2DataOk=true;
            }
            else
            {
                isLaser2DataOk=false;
            }

            if(true==dataOut.issuc && true==isLaser1DataOk && true==isLaser2DataOk)
            {
                posCalculate->transSensorData2VehiclePosition(laser1Data,laser2Data,imageOffsetData,posOut);
//                laserRequestCount=0;
                imageRequestCount=0;
                return 1;
            }
            else
            {
                qDebug()<<"dataOut.issuc==false,errorcode="<<dataOut.errcode;
            }

        }
    }
    return -1;
}

int Cu1SensorAdapter::process_manual_allSensorData(QJsonObject &redeiveJsonObjIn,SensorRawData &dataOut)
{

    dataOut.issuc=redeiveJsonObjIn.value("issuc").toBool();
    dataOut.errcode=redeiveJsonObjIn.value("errcode").toInt();
    QJsonObject subObj = redeiveJsonObjIn.value("rtval").toObject();
    dataOut.d1=subObj["d1"].toDouble();
    dataOut.d2 =subObj["d2"].toDouble();
    if(false==subObj.contains("offsetx"))
    {
       qDebug()<<"json not contain offsetx";
       return -1;
    }
    dataOut.offsetx=subObj["offsetx"].toDouble();
    return 1;
}
